///\todo upgrade

//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#ifndef _BUOLA_MAT_C3DROTTRANS_H_
#define _BUOLA_MAT_C3DROTTRANS_H_

#include <buola/mat/cmat.h>
#include <buola/mat/cquat.h>

namespace buola { namespace mat {

///\addtogroup geometry
///@{

////////////////////////////////////////////////////////////////////////////

/// rotation in 3D

/// Represents a rotation in 3 dimensions, as yaw, pitch and roll angles.
/// This representation is quite intuitive, but it can't be directly used
/// to rotate vectors, so it has to be converted to a matrix or quaternion.
///
////////////////////////////////////////////////////////////////////////////

template<typename tScalar>
class C3DRotTrans
{
public:
//! \name constructor and destructor
//@{
    //! default constructor (initialises to identity transformation)
    C3DRotTrans()
        :   mRot()
        ,   mTrans(0,0,0)
    {}

    //! copy constructor
    C3DRotTrans(const C3DRotTrans &pO)=default;

    //! from rotation and (optionally) translation
    C3DRotTrans(const CQuat<tScalar> &pRot,const CVec<tScalar,3> &pTrans=(CVec<tScalar,3>(0,0,0)))
        :   mRot(pRot)
        ,   mTrans(pTrans)
    {}

    //! from translation
    C3DRotTrans(const CVec<tScalar,3> &pTrans)
        :   mTrans(pTrans)
    {}

    mat::CQuat_d &Rotation()                        {   return mRot;    }
    const mat::CQuat_d &Rotation() const            {   return mRot;    }
    mat::CVec<tScalar,3> &Translation()             {   return mTrans;  }
    const mat::CVec<tScalar,3> &Translation() const {   return mTrans;  }
    
//@}

    //! assignment operator
    C3DRotTrans &operator=(const C3DRotTrans &pRH)=default;
    
    C3DRotTrans operator*(const C3DRotTrans &pRH)
    {
        return C3DRotTrans(mRot*pRH.mRot,mRot*pRH.mTrans+mTrans);
    }
    
    friend C3DRotTrans inverse(const C3DRotTrans &pT)
    {
        CQuat<tScalar> lRot=conj(pT.mRot);
        return C3DRotTrans(lRot,-lRot*pT.mTrans);
    }
    
private:
    CQuat<tScalar> mRot;
    CVec<tScalar,3> mTrans;
};

typedef C3DRotTrans<double> C3DRotTrans_d;

///@}

/*namespace mat*/ } /*namespace buola*/ }

#endif
